﻿using System;
using System.Linq;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using System.Threading;

using RobotCtrl;
using RobotView;

namespace Testat_2_HS12
{
    public enum LedStates
    {
        Disable = 0,
        Flashing,
        Enable
    }

    public partial class Form1 : Form
    {
        #region constants
        private const float defaultAcceleration = 0.3f;
        private const float defaultSpeed = 0.5f;
        private const int defaultLedFlashingTime_ms = 400;
        #endregion 

        #region members
        private Robot robot;
        private Switches swi; // switches
        private bool swiState;
        private LedStates ledState; // led state selector
        private Thread driveTh; // drive Thread, start after switch changes
        private Thread speedTh; // speed handler, start during initialize object
        private Thread ledTh; // led handler, start during initialize object
        #endregion

        #region constructor & destructor
        public Form1()
        {
            InitializeComponent();
            robot = new Robot();

            // enable power
            robot.Drive.Power = true;

            // link needed Drive components
            driveView1.Drive = robot.Drive;
            runLine1.Drive = robot.Drive;
            runTurn1.Drive = robot.Drive;
            runArcLeftRight1.Drive = robot.Drive;

            // for debuging
            consoleView1.RobotConsole = robot.RobotConsole;
            radarView1.Radar = robot.Radar;

            // register Switches
            robot.RobotConsole[Switches.Switch1].SwitchStateChanged += SwitchStateChanged;
            robot.RobotConsole[Switches.Switch2].SwitchStateChanged += SwitchStateChanged;
            robot.RobotConsole[Switches.Switch3].SwitchStateChanged += SwitchStateChanged;
            robot.RobotConsole[Switches.Switch4].SwitchStateChanged += SwitchStateChanged;

            // set default state and start infinite led handler
            ledTh = new Thread(LedHandler);
            ledTh.Start();

            // set default state and start infinite speed handler
            speedTh = new Thread(SpeedHandler);
            speedTh.Start();
        }
        #endregion 

        #region properties
        private float CurrentSpeed {
            set {
                runLine1.Speed = value;
                runTurn1.Speed = value;
                runArcLeftRight1.Speed = value;
            }
        }

        private float CurrentAcceleration {
            set {
                runLine1.Acceleration = value;
                runTurn1.Acceleration = value;
                runArcLeftRight1.Acceleration = value;
            }
        }
        #endregion 

        #region methods

        private void LedHandler()
        {
            Leds led; // led selector
            ledState = LedStates.Disable; // start with defined state

            while (true) // infinite loop
            {
                led = (Leds)(int)swi;
                switch (ledState)
                {
                    case LedStates.Disable :
                        // disable all led's
                        robot.RobotConsole[Leds.Led1].LedEnabled = false;
                        robot.RobotConsole[Leds.Led2].LedEnabled = false;
                        robot.RobotConsole[Leds.Led3].LedEnabled = false;
                        robot.RobotConsole[Leds.Led4].LedEnabled = false;
                        Thread.Sleep(100); // we don't need cpu ressources for 100 ms
                        break;
                    case LedStates.Flashing :
                        robot.RobotConsole[led].LedEnabled = true;
                        Thread.Sleep(defaultLedFlashingTime_ms >> 1);
                        robot.RobotConsole[led].LedEnabled = false;
                        Thread.Sleep(defaultLedFlashingTime_ms >> 1);
                        break;
                    case LedStates.Enable :
                        robot.RobotConsole[led].LedEnabled = true;
                        Thread.Sleep(100); // we don't need cpu ressources for 100 ms
                        break;
                    default :
                        ledState = LedStates.Disable;
                        break;
                }
            }
        }

        private void SpeedHandler()
        {
            // start with defined properties
            CurrentAcceleration = defaultAcceleration;
            CurrentSpeed = defaultSpeed;
            while (true) // infinite loop
            {
                if (robot.Radar.Distance > 0.1f)
                { // more than 0.1 m distance
                    CurrentSpeed = defaultSpeed;
                } else {
                    // stop robot!
                    // CurrentSpeed = 0.1f;
                    robot.Drive.Stop();
                }
                Thread.Sleep(10); // sleep 10 ms
            }
        }

        private void DriveHandler()
        {
            Thread RouteTh;
            if (!swiState)
            {
                ledState = LedStates.Disable;
                /* no more actions */
            }
            else
            {

                ledState = LedStates.Flashing;
                switch (swi)
                {
                    case Switches.Switch1:
                        RouteTh = new Thread(DriveRouteHandler_1);
                        break;
                    case Switches.Switch2:
                        RouteTh = new Thread(DriveRouteHandler_2);
                        break;
                    case Switches.Switch3:
                        RouteTh = new Thread(DriveRouteHandler_3);
                        break;
                    case Switches.Switch4:
                        RouteTh = new Thread(DriveRouteHandler_4);
                        break;
                    default:
                        /* error */
                        RouteTh = null;
                        return;
                }
                RouteTh.Start();
                RouteTh.Join();
                ledState = LedStates.Enable;
            }
        }

        private void DriveRouteHandler_1()
        {
            robot.Drive.RunTurn(-45.0f, runTurn1.Speed, runTurn1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunLine(1.1f, runLine1.Speed, runLine1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunTurn(45.0f, runTurn1.Speed, runTurn1.Acceleration); 
            while (!robot.Drive.Done) ;
            robot.Drive.RunLine(1.1f, runLine1.Speed, runLine1.Acceleration);
            while (!robot.Drive.Done) ;
        }

        private void DriveRouteHandler_2()
        {
            robot.Drive.RunTurn(-45.0f, runTurn1.Speed, runTurn1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunArcRight(.75f, 45.0f, runArcLeftRight1.Speed, runArcLeftRight1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunLine(1.4f, runLine1.Speed, runLine1.Acceleration);
            while (!robot.Drive.Done) ;
        }

        private void DriveRouteHandler_3()
        {
            robot.Drive.RunTurn(45.0f, runTurn1.Speed, runTurn1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunArcLeft(.75f, 45.0f, runArcLeftRight1.Speed, runArcLeftRight1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunLine(1.4f, runLine1.Speed, runLine1.Acceleration);
            while (!robot.Drive.Done) ;
        }

        private void DriveRouteHandler_4()
        {
            robot.Drive.RunTurn(45.0f, runTurn1.Speed, runTurn1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunLine(1.1f, runLine1.Speed, runLine1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunTurn(-45.0f, runTurn1.Speed, runTurn1.Acceleration);
            while (!robot.Drive.Done) ;
            robot.Drive.RunLine(1.1f, runLine1.Speed, runLine1.Acceleration);
            while (!robot.Drive.Done) ;
        }

        void SwitchStateChanged(object sender, SwitchEventArgs e)
        {
            swi = e.Swi; // Event-Source
            swiState = e.SwitchEnabled;
            driveTh = new Thread(DriveHandler);
            driveTh.Start();
        }

        #endregion 
    }
}